#include "comm.hpp"

#include <esp_now.h>
//#include ""

#include <WiFi.h>
//#include <WiFi.h>
#include<Arduino.h>
#include"../Preferences/Preferences.h"

const int peerNum = 32;
int usingPeerIdx = 0;
esp_now_peer_info_t peerInfo[peerNum];
char CommBuffer[250];
uint8_t bufferFilled = 0;
const int addressSize = 6;

Preferences preferencesComm;

//#define ESP_NOW_VERBOSE 

void OnDataSent(const uint8_t* mac_addr, esp_now_send_status_t status) {
#ifdef ESP_NOW_VERBOSE
	Serial.println("Sent over NOW");
#endif
}

void OnDataRecv(const uint8_t* mac, const uint8_t* incomingData, int len) {
	if (bufferFilled == 0) {
		memcpy(CommBuffer, incomingData, len);
#ifdef ESP_NOW_VERBOSE
		Serial.println("Received over NOW");
#endif
		bufferFilled = len;
	}
	else {
#ifdef ESP_NOW_VERBOSE
		Serial.println("Received congustion");
#endif
	}
}

void savePeer(uint8_t* mac) {
	Serial.println("Saving peers");
	if (preferencesComm.begin("COMM", false)) {
		int putLen = preferencesComm.putBytes("Peer0", mac, 6);
		if (putLen == 6) {
			Serial.println("Peer 0 saved");
		}
		else {
			Serial.print("Save peer error: ");
			Serial.println(putLen);
		}
	}
	else {
		Serial.println("No config");
	}
	preferencesComm.end();
}

byte peerBuffer[6];
void loadPeers() {
	Serial.println("Loading peers");
	if (preferencesComm.begin("COMM", true)) {
		int readLen = preferencesComm.getBytes("Peer0", peerBuffer, 6);
		if (readLen == 6) {
			Comm::addPeer(peerBuffer, false);
			Serial.println("Load peer 0: ");
			for (int idx = 0; idx < 6; idx++) {
				Serial.print(peerBuffer[idx], HEX);
				Serial.print(",");
			}
			Serial.println("");
		}
	}
	else {
		Serial.println("No config");
	}
	preferencesComm.end();
}

int loadStartupMode() {
	int ret = 0;
	Serial.print("Loading startup mode ");
	if (preferencesComm.begin("COMM", true)) {
		ret = preferencesComm.getUChar("StartUp", ret);
		Serial.println(ret);
	}
	else {
		Serial.println("No config");
	}
	preferencesComm.end();
	return ret;
}

int Comm::init() {
	Serial.begin(115200);

	// Set device as a Wi-Fi Station
	WiFi.mode(WIFI_STA);
	Serial.println("Wifi mac");
	Serial.println(WiFi.macAddress());

	// Init ESP-NOW
	if (esp_now_init() != ESP_OK) {
		Serial.println("Error initializing ESP-NOW");
		return 0;
	}

	esp_now_register_send_cb(OnDataSent);
	esp_now_register_recv_cb(OnDataRecv);

	loadPeers();
	return loadStartupMode();
}

void Comm::setStartupMode(int val) {
	Serial.print("Saving startup mode ");
	Serial.println(val);
	if (preferencesComm.begin("COMM", false)) {
		preferencesComm.putUChar("StartUp", val);
	}
	else {
		Serial.println("No config");
	}
	preferencesComm.end();
}

int Comm::addPeer(uint8_t* address, bool save) {
	if (usingPeerIdx < peerNum) {
		int pIdx = usingPeerIdx;
		usingPeerIdx++;
		memcpy(peerInfo[pIdx].peer_addr, address, addressSize);
		peerInfo[pIdx].channel = 0;
		peerInfo[pIdx].encrypt = false;

		auto flag = esp_now_add_peer(peerInfo + pIdx);
		if (flag != ESP_OK) {
			Serial.print("Failed to add peer # ");
			Serial.print(pIdx);
			Serial.print(": ");
			Serial.println(flag);
			return -1;
		}

		if (save) {
			savePeer(address);
		}

		return pIdx;
	}
	return -2;
}

void Comm::sendStr(const char* msg) {
	sendStr(0, msg);
}

int Comm::sendStr(int idx, const char* msg) {
	esp_err_t result = esp_now_send(peerInfo[idx].peer_addr, (uint8_t*)msg, strlen(msg) + 1);
#ifdef ESP_NOW_VERBOSE
	if (result == ESP_OK) {
		Serial.println("Sent with success");
	}
	else {
		Serial.println("Error sending the data");
	}
#endif
	return result;
}

int Comm::receiveStr(char* msg) {
	if (bufferFilled == 0) {
		return 0;
	}
	int ret = bufferFilled;
	memcpy(msg, CommBuffer, bufferFilled);
	bufferFilled = 0;
	return ret;
}
